#include <stddef.h>
#include <pthread.h>
#include <semaphore.h>

#include "octhreadpool.h"
//#include "ocexception.h"

struct _oc_thread_pool_t
{
    gulong                 nthreads;
    gulong                 nthreads_done;
    gulong                 nthreads_terminated;

    pthread_t               *thread;
    oc_usr_thread_func_t    usr_thread_func;

    sem_t                   *run_sem;
    sem_t                   barrier_sem;
    gboolean                *be_terminated;
    sem_t                   termination_finished_sem;

    GAsyncQueue             **queue;

    oc_task_range_t           *task_range;
    /*gpointer                *usr_task_set;
    gulong                 ntasks;*/
};

typedef struct
{
    gulong                 *nthreads;
    gulong                 *nthreads_done;
    gulong                 *nthreads_terminated;

    gulong                 thread_index;
    oc_usr_thread_func_t    *usr_thread_func;

    sem_t                   *run_sem;
    sem_t                   *barrier_sem;
    gboolean                *be_terminated;
    sem_t                   *termination_finished_sem;

    GAsyncQueue             **queue;

    oc_task_range_t           **task_range;
    /*gpointer                **usr_task_set;
    gulong                 *ntasks;*/
}oc_thread_arg_t;

/*
*
*
typedef struct
{
    gpointer        *data;
}oc_task_t;
*/

static pthread_key_t queue_index_key;
static void *oc_thread_func( gpointer arg );

oc_thread_pool_t *oc_thread_pool_new (
                            gulong nthreads
                            //oc_usr_thread_func_t usr_func,
                            )
                            //oc_task_range_t *task_range )
                            //gpointer *usr_task_set ,
                            //gulong ntasks )
{
    oc_thread_pool_t *tpool = g_slice_alloc ( sizeof ( oc_thread_pool_t ) );
    oc_thread_pool_set_num_threads( tpool, nthreads );
    tpool->nthreads_done = 0;
    tpool->nthreads_terminated = 0;

    //tpool->thread = g_new ( pthread_t, tpool->nthreads );
    tpool->thread = g_slice_alloc ( sizeof ( pthread_t ) * tpool->nthreads );
    //oc_thread_pool_set_usr_func( tpool, usr_func );
    oc_thread_pool_set_usr_func( tpool, NULL );

    //tpool->run_sem = g_new ( sem_t, tpool->nthreads );
    tpool->run_sem = g_slice_alloc ( sizeof ( sem_t ) * tpool->nthreads );
    sem_init ( & tpool->barrier_sem, 0, 0 );
    //tpool->be_terminated = g_new ( gboolean, tpool->nthreads );
    tpool->be_terminated = g_slice_alloc ( sizeof ( gboolean ) * tpool->nthreads );
    sem_init ( & tpool->termination_finished_sem, 0, 0 );

    //tpool->queue = g_new ( GAsyncQueue *, tpool->nthreads );
    tpool->queue = g_slice_alloc ( sizeof ( GAsyncQueue * ) * tpool->nthreads );

    //oc_thread_pool_set_task_range ( tpool, task_range );
    oc_thread_pool_set_task_range ( tpool, NULL );
    /*oc_thread_pool_set_tasks_entry  (  tpool,
                                        usr_task_set ,
                                        ntasks );*/
    pthread_key_create ( & queue_index_key, NULL );

    pthread_attr_t attr;
    pthread_attr_init( & attr );
    pthread_attr_setdetachstate( & attr, PTHREAD_CREATE_DETACHED );
    pthread_attr_setscope( & attr, PTHREAD_SCOPE_SYSTEM );
    oc_thread_arg_t *arg;
    gulong i;
    for ( i = 0; i != tpool->nthreads; ++i )
    {
        sem_init ( & tpool->run_sem[i], 0, 0 );
        tpool->be_terminated[i] = TRUE;
        tpool->queue[i] = g_async_queue_new();

        //arg = g_new( oc_thread_arg_t, 1 );
        arg = g_slice_alloc ( sizeof ( oc_thread_arg_t ) );
        arg->nthreads = & tpool->nthreads;
        arg->nthreads_done = & tpool->nthreads_done;
        arg->nthreads_terminated = & tpool->nthreads_terminated;
        arg->thread_index = i;
        arg->usr_thread_func = & tpool->usr_thread_func;

        arg->run_sem = & tpool->run_sem[i];
        arg->barrier_sem = & tpool->barrier_sem;
        arg->be_terminated = & tpool->be_terminated[i];
        arg->termination_finished_sem = & tpool->termination_finished_sem;
        arg->queue = tpool->queue;
        arg->task_range = & tpool->task_range;
        //arg->usr_task_set = & tpool->usr_task_set;
        //arg->ntasks = & tpool->ntasks;

        pthread_create( & tpool->thread[i], & attr, oc_thread_func, arg );
    }
    pthread_attr_destroy( & attr );
    return tpool;

}

void oc_thread_pool_set_num_threads (   oc_thread_pool_t *tpool,
                                        gulong nthreads )
{/*
    if ( nthreads == OC_THREAD_POOL_DEFAULT_NUM_THREADS )
    {
        tpool->nthreads = sysconf ( _SC_NPROCESSORS_ONLN );
    }
    else
    {
        tpool->nthreads = nthreads;
    }
    */
    tpool->nthreads = nthreads;
}

void oc_thread_pool_set_usr_func    (   oc_thread_pool_t *tpool,
                                    oc_usr_thread_func_t usr_func )
{
    tpool->usr_thread_func = usr_func;
}

void oc_thread_pool_set_task_range  (   oc_thread_pool_t *tpool,
                                        oc_task_range_t *task_range )
                                        //gpointer *usr_task_set ,
                                        //gulong ntasks )
{
    //tpool->usr_task_set = usr_task_set;
    //tpool->ntasks = ntasks;
    tpool->task_range = task_range;
}

void oc_thread_pool_fireup  ( oc_thread_pool_t *tpool
                                )
{
    tpool->nthreads_done = 0;
    gulong i;
    for ( i = 0; i < tpool->nthreads; ++i )
    {
        if ( tpool->be_terminated[i] )
        {
            tpool->be_terminated[i] = FALSE;
            sem_post ( & tpool->run_sem[i] );
        }
    }
}

gulong oc_thread_get_queue_index ()
{
    return ( gulong ) pthread_getspecific ( queue_index_key );
}

static void *oc_thread_func( gpointer arg )
{
    oc_thread_arg_t *thread_arg = ( oc_thread_arg_t * ) arg;

    gulong *nthreads = thread_arg->nthreads;
    gulong *nthreads_done = thread_arg->nthreads_done;
    gulong *nthreads_terminated = thread_arg->nthreads_terminated;

    gulong thread_index = thread_arg->thread_index;
    //pthread_setspecific ( thread_index_key, ( void * ) thread_index );
    oc_usr_thread_func_t *usr_thread_func = thread_arg->usr_thread_func;

    sem_t *run_sem = thread_arg->run_sem;
    sem_t *barrier_sem = thread_arg->barrier_sem;
    gboolean *be_terminated = thread_arg->be_terminated;
    sem_t *termination_finished_sem = thread_arg->termination_finished_sem;

    GAsyncQueue **queue = thread_arg->queue;

    oc_task_range_t **task_range = thread_arg->task_range;
    //gpointer **usr_task_set = thread_arg->usr_task_set;
    //gulong *ntasks = thread_arg->ntasks;

    g_slice_free1 ( sizeof ( oc_thread_arg_t ) , thread_arg );

    while ( 1 )
    {
        /*
        * Wait until the pool is fired up
        **/
        sem_wait ( run_sem );
        /*
        * Just for the destruction of the pool
        * To terminate this thread by returning NULL
        **/
        if ( * be_terminated )
        {
            break;
        }

        /*
        * Read related tasks in the task set to the queue
        **/
        gulong i = thread_index;
        gpointer task = NULL;
        gulong ntasks = oc_task_range_get_num_tasks ( * task_range );
        while ( i < ntasks )
        {
            //task = g_new ( oc_task_t, 1 );
            //task->data = ( * usr_task_set )[i];
            task = oc_task_range_get_task ( * task_range, i );
            g_async_queue_push ( queue[thread_index],
                                            task );
            i += ( * nthreads );
        }
        /*
        * Do every task bufferd in the queue until
        * there is no tasks in it
        * Then the thread will steal just one task
        * from other queues, if possible
        **/
        while ( 1 )
        {
            if ( ( task = g_async_queue_try_pop ( queue[thread_index] ) ) )
            {
                pthread_setspecific ( queue_index_key, ( void * ) thread_index );
                ( * usr_thread_func ) ( task );

            }//if
            else
            {
                for (   i = ( thread_index + 1 ) % ( * nthreads );
                        i != thread_index;
                        i = ( i + 1 ) % ( * nthreads ) )
                {
                    if ( ( task = g_async_queue_try_pop ( queue[thread_index] ) ) )
                    {
                        pthread_setspecific ( queue_index_key, ( void * ) i );
                        ( * usr_thread_func ) ( task );
                        break;
                    }
                }//for
                if ( i == thread_index )
                {
                    g_atomic_int_inc( ( gint * ) nthreads_done );
                    if ( ( gulong ) g_atomic_int_get ( nthreads_done ) == *nthreads )
                    {
                        sem_post ( barrier_sem );
                    }
                    break;
                }//if

            }//else

        }//while

    }//while

    g_atomic_int_inc( ( gint * ) nthreads_terminated );
    if ( ( gulong ) g_atomic_int_get ( nthreads_terminated ) == *nthreads )
    {
        sem_post ( termination_finished_sem );
    }
    return NULL;
}

void oc_thread_pool_barrier( oc_thread_pool_t *tpool )
{
    sem_wait ( & tpool->barrier_sem );
}

void oc_thread_pool_destroy( oc_thread_pool_t *tpool )
{
    gulong i;
    for ( i = 0; i < tpool->nthreads; ++i )
    {
        if ( ! tpool->be_terminated[i] )
        {
            tpool->be_terminated[i] = TRUE;
            sem_post ( & tpool->run_sem[i] );
        }
    }
    sem_wait ( & tpool->termination_finished_sem );

    for ( i = 0; i != tpool->nthreads; ++i )
    {
        g_async_queue_unref ( tpool->queue[i] );
        sem_destroy ( & tpool->run_sem[i] );
    }
    g_slice_free1 ( sizeof ( GAsyncQueue * ) * tpool->nthreads, tpool->queue );
    sem_destroy ( & tpool->termination_finished_sem );
    g_slice_free1 ( sizeof ( gboolean ) * tpool->nthreads, tpool->be_terminated );
    sem_destroy ( & tpool->barrier_sem );
    g_slice_free1 ( sizeof ( sem_t ) * tpool->nthreads, tpool->run_sem );
    g_slice_free1 ( sizeof ( pthread_t ) * tpool->nthreads, tpool->thread );
    g_slice_free1 (  sizeof ( oc_thread_pool_t ), tpool );

}
